/*******************************************************************************
*
*   Copyright ©2009 Alexandru Marin
*
*   This file is part of nwave/pMRI C library.
*
*   This project started as an MSc degree project during a
*   stage at the Paris-Est university, under joint supervision of prof.
*   Bogdan Ionescu (Image Analysis and Processing Laboratory, "Politehica"
*   University of Bucharest), Caroline Chaux and Jean-Christophe Pesquet
*   (Laboratoire d'Informatique, Université Paris-Est, Institut Gaspard Monge).
*
*   nwave/pMRI C libray is free software: you can redistribute it and/or modify
*   it under the terms of the GNU General Public License as published by
*   the Free Software Foundation, either version 2 of the License, or
*   (at your option) any later version.
*
*   nwave/pMRI C libray is distributed in the hope that it will be useful,
*   but WITHOUT ANY WARRANTY; without even the implied warranty of
*   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
*   GNU General Public License for more details.
*
*   You should have received a copy of the GNU General Public License
*   along with nwave/pMRI C library.  If not, see <http://www.gnu.org/licenses/>.
*
******************************************************************************/


#include "mri_noise.h"
#include "mri_types.h"
#include "mri_utils.h"
#include "mri_gsl_bridge.h"
#include "gsl/gsl_blas.h"
#include "gsl/gsl_cblas.h"
#include "gsl/gsl_types.h"
#include "gsl/gsl_linalg.h"

mri_image_t* mri_recons_ls(
		mri_image_t** images,
		mri_sens_t* sens,
		int R,
		float lambda) {

	// Image dimensions into local variables
	int nsamples = images[0]->nsamples;
	int nslices = images[0]->nslices;
	int redheight = images[0]->height;
	int fullheight = R * images[0]->height;
	int width = images[0]->width;
	int ncoils = sens->ncoils;

//	printf("* MRI: nsamples = %d, nslices = %d, redheight = %d, fullheight = %d, width = %d, ncoils = %d \n",
//			nsamples, nslices, redheight, fullheight, width, ncoils);

	// Loop iterators
	int samplenum;
	int slicenum;
	int rownum;
	int colnum;
	int coilnum;
	int r;

//	printf("Allocating output image...\n");
	mri_image_t* result;
	result = mri_image_alloc(fullheight, width, nslices, nsamples);

	MRI_IMAGE(result, 255, 255, 0, 0) = 0;

//	printf("Allocating working variables...\n");
	gsl_vector_complex *dx = gsl_vector_complex_alloc(ncoils);
	gsl_matrix_complex *sx = gsl_matrix_complex_alloc(ncoils, R);
	gsl_vector_complex *rhox = gsl_vector_complex_alloc(R);
	gsl_matrix_complex *inverse = gsl_matrix_complex_alloc(R, R);
	gsl_matrix_complex *tmpm = gsl_matrix_complex_alloc(R, R);
	gsl_vector_complex *tmpv = gsl_vector_complex_alloc(R);
	gsl_permutation *perm = gsl_permutation_alloc (R);
	int signum;
	gsl_complex one; GSL_REAL(one) = 1; GSL_IMAG(one) = 0;
	gsl_complex zero; GSL_REAL(zero) = 0; GSL_IMAG(zero) = 0;
	gsl_complex gsl_lambda; GSL_REAL(gsl_lambda) = lambda; GSL_IMAG(gsl_lambda) = 0;

//	printf("Iterating blocks...\n");
	// Iterate through time dimension
	for (samplenum = 0; samplenum < nsamples; samplenum++) {
//		printf("* * * sample #%d\n", samplenum);
		// Iterate through image slices (depth)
		for (slicenum = 0; slicenum < nslices; slicenum++) {
//			printf("* * * slice #%d\n", slicenum);
			// Iterate  through the rows of the new images
			for (rownum = 0; rownum < redheight; rownum++) {
//				printf("* * * row #%d\n", rownum);fflush(stdout);
				// Iterate through columns
				for (colnum = 0; colnum < width; colnum++) {
//					printf("* * * col #%d\n", colnum);fflush(stdout);

					// Fill in the dx
					for (coilnum = 0; coilnum < sens->ncoils; coilnum++)
						gsl_vector_complex_set(dx, coilnum,
								mri_complex_to_gsl(MRI_IMAGE(images[coilnum], rownum, colnum, slicenum, samplenum)));

					if(rownum == 32 && colnum == 128) {
						printf("----- d(x) -----\n");
						gsl_vector_complex_fprintf(stdout, dx, "%f");
					}

					// Fill in the sx (maybe there is some room for optimisation
					// here by using a gls_matrix_view on the memory space of S)
					for (coilnum = 0; coilnum < sens->ncoils; coilnum++)
						for (r = 0; r < R; r++)
							gsl_matrix_complex_set(sx, coilnum, r,
								mri_complex_to_gsl(MRI_SENS(sens, rownum + r * redheight, colnum, coilnum, slicenum)));

					if(rownum == 32 && colnum == 128) {
						printf("----- S(x) -----\n");
						gsl_matrix_complex_fprintf(stdout, sx, "%f");
					}


					// Reconstruct block //

//					printf("Computing tmpm = S(x)S^H(x) + \\lambda*Id... \n");
					gsl_matrix_complex_set_identity (tmpm);
					gsl_blas_zgemm (CblasConjTrans, CblasNoTrans, one, sx, sx, gsl_lambda, tmpm);

//					printf("Computing tmpv = S^H(x)d(x)... \n");
					gsl_blas_zgemv (CblasConjTrans, one, sx, dx, zero, tmpv);

//					printf("Computing tmp = (S(x)S^H(x) + \\lambda*Id)^{-1}S^H(x)d(x)... \n");fflush(stdout);
					gsl_linalg_complex_LU_decomp (tmpm, perm, &signum);
					gsl_linalg_complex_LU_invert (tmpm, perm, inverse);
					gsl_blas_zgemv (CblasNoTrans, one, inverse, tmpv, zero, rhox);


//					printf("Copying to result.../n");
					//(same optimisation applies here)
					for (r = 0; r < R; r++) {
						MRI_IMAGE(result, rownum + r * redheight, colnum, slicenum, samplenum) =
							mri_complex_from_gsl(gsl_vector_complex_get(rhox, r));
//						printf("MRI_IMAGE(%d, %d, %d, %d, %d) = %f + %f i \n",
//								result, rownum + r * redheight, colnum, slicenum, samplenum,
//								crealf(mri_complex_from_gsl(gsl_vector_complex_get(rhox, r))),
//								cimagf(mri_complex_from_gsl(gsl_vector_complex_get(rhox, r))));
					}

				}
			}
		}
	}

	return result;
}
